home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / npend / maple / npend.f < prev    next >
Text File  |  1999-09-16  |  13KB  |  418 lines
  1. c      
  2. c     SUBROUTINE npend
  3. c      
  4.       subroutine npend(neq,t,th,ydot)
  5.         parameter (n=10)
  6.         implicit doubleprecision (t)
  7.         doubleprecision t,th(2*n),ydot(2*n),r(n),j(n),m(n)
  8.         doubleprecision me3s(n,n),cc3s(n,n),const(n,1)
  9.         doubleprecision w(3*n),rcond    
  10.  
  11.         integer i,k,neq,ierr
  12.         data g / 9.81/
  13.         data r / n*1.0/
  14.         data m / n*1.0/
  15.         data j / n*0.3/
  16.       t1 = -th(9)
  17.       t4 = 2*m(10)
  18.       t5 = m(9)+t4
  19.       t9 = 2*r(7)*r(9)*cos(th(7)+t1)*t5
  20.       t15 = 2*r(3)*r(9)*cos(th(3)+t1)*t5
  21.       t16 = -th(8)
  22.       t19 = 2*m(9)
  23.       t20 = m(8)+t4+t19
  24.       t24 = 2*r(7)*r(8)*cos(th(7)+t16)*t20
  25.       t25 = r(2)**2
  26.       t44 = -th(6)
  27.       t47 = 2*m(7)
  28.       t48 = 2*m(8)
  29.       t49 = m(6)+t19+t4+t47+t48
  30.       t53 = 2*r(5)*r(6)*cos(th(5)+t44)*t49
  31.       t59 = 2*r(6)*r(9)*cos(th(6)+t1)*t5
  32.       t60 = -th(10)
  33.       t66 = 2*r(5)*m(10)*r(10)*cos(th(5)+t60)
  34.       t72 = 2*r(1)*r(8)*cos(th(1)+t16)*t20
  35.       t73 = -th(4)
  36.       t76 = 2*m(5)
  37.       t77 = 2*m(6)
  38.       t78 = t48+m(4)+t19+t76+t47+t4+t77
  39.       t82 = 2*r(3)*r(4)*cos(th(3)+t73)*t78
  40.       t88 = 2*r(8)*r(9)*cos(th(8)+t1)*t5
  41.       t89 = r(5)**2
  42.       t102 = -th(7)
  43.       t105 = t4+m(7)+t48+t19
  44.       t109 = 2*r(6)*r(7)*cos(th(6)+t102)*t105
  45.       t115 = 2*r(9)*m(10)*r(10)*cos(th(9)+t60)
  46.       t116 = r(3)**2
  47.       t138 = 2*r(4)*r(9)*cos(th(4)+t1)*t5
  48.       t144 = 2*r(3)*m(10)*r(10)*cos(th(3)+t60)
  49.       t150 = 2*r(4)*m(10)*r(10)*cos(th(4)+t60)
  50.       t151 = -th(3)
  51.       t154 = 2*m(4)
  52.       t155 = t76+t154+t4+t48+t47+m(3)+t19+t77
  53.       t159 = 2*r(1)*r(3)*cos(th(1)+t151)*t155
  54.       t160 = r(8)**2
  55.       t167 = r(6)**2
  56.       t183 = 2*r(6)*m(10)*r(10)*cos(th(6)+t60)
  57.       t184 = r(10)**2
  58.       t192 = 2*r(2)*r(4)*cos(th(2)+t73)*t78
  59.       t193 = r(9)**2
  60.       t198 = r(7)**2
  61.       t212 = 2*r(3)*r(6)*cos(th(3)+t44)*t49
  62.       t218 = 2*r(2)*r(3)*cos(th(2)+t151)*t155
  63.       t224 = 2*r(5)*r(9)*cos(th(5)+t1)*t5
  64.       t230 = 2*r(5)*r(7)*cos(th(5)+t102)*t105
  65.       t236 = 2*r(7)*m(10)*r(10)*cos(th(7)+t60)
  66.       t242 = 2*r(1)*r(4)*cos(th(1)+t73)*t78
  67.       t248 = 2*r(1)*r(6)*cos(th(1)+t44)*t49
  68.       t254 = 2*r(2)*r(6)*cos(th(2)+t44)*t49
  69.       t260 = 2*r(2)*m(10)*r(10)*cos(th(2)+t60)
  70.       t266 = 2*r(8)*m(10)*r(10)*cos(th(8)+t60)
  71.       t267 = -th(5)
  72.       t270 = t77+t48+t19+t47+m(5)+t4
  73.       t274 = 2*r(1)*r(5)*cos(th(1)+t267)*t270
  74.       t280 = 2*r(4)*r(5)*cos(th(4)+t267)*t270
  75.       t286 = 2*r(4)*r(7)*cos(th(4)+t102)*t105
  76.       t292 = 2*r(6)*r(8)*cos(th(6)+t16)*t20
  77.       t298 = 2*r(1)*r(9)*cos(th(1)+t1)*t5
  78.       t304 = 2*r(5)*r(8)*cos(th(5)+t16)*t20
  79.       t310 = 2*r(3)*r(5)*cos(th(3)+t267)*t270
  80.       t316 = 2*r(3)*r(7)*cos(th(3)+t102)*t105
  81.       t322 = 2*r(2)*r(7)*cos(th(2)+t102)*t105
  82.       t331 = 2*r(1)*r(2)*cos(th(1)-th(2))*(2*m(3)+m(2)+t154+t47+t4+t76+t
  83.      +77+t19+t48)
  84.       t332 = r(4)**2
  85.       t352 = 2*r(4)*r(8)*cos(th(4)+t16)*t20
  86.       t358 = 2*r(2)*r(5)*cos(th(2)+t267)*t270
  87.       t364 = 2*r(2)*r(9)*cos(th(2)+t1)*t5
  88.       t370 = 2*r(2)*r(8)*cos(th(2)+t16)*t20
  89.       t376 = 2*r(1)*r(7)*cos(th(1)+t102)*t105
  90.       t382 = 2*r(3)*r(8)*cos(th(3)+t16)*t20
  91.       t388 = 2*r(4)*r(6)*cos(th(4)+t44)*t49
  92.       t389 = r(1)**2
  93.       t417 = 2*r(1)*m(10)*r(10)*cos(th(1)+t60)
  94.          me3s(9,7) = t9
  95.          me3s(3,9) = t15
  96.          me3s(8,7) = t24
  97.          me3s(2,2) = J(2)+4*t25*m(3)+4*t25*m(4)+4*t25*m(7)+4*t25*m(8)+m(
  98.      +2)*t25+4*t25*m(9)+4*t25*m(5)+4*t25*m(10)+4*t25*m(6)
  99.          me3s(6,5) = t53
  100.          me3s(9,6) = t59
  101.          me3s(10,5) = t66
  102.          me3s(8,1) = t72
  103.          me3s(4,3) = t82
  104.          me3s(9,8) = t88
  105.          me3s(5,5) = 4*t89*m(8)+4*t89*m(10)+4*t89*m(7)+4*t89*m(9)+4*t89*
  106.      +m(6)+J(5)+m(5)*t89
  107.          me3s(7,6) = t109
  108.          me3s(10,9) = t115
  109.          me3s(3,3) = 4*t116*m(4)+4*t116*m(10)+4*t116*m(9)+4*t116*m(5)+4*
  110.      +t116*m(6)+m(3)*t116+J(3)+4*t116*m(7)+4*t116*m(8)
  111.          me3s(4,9) = t138
  112.          me3s(3,10) = t144
  113.          me3s(10,4) = t150
  114.          me3s(3,1) = t159
  115.          me3s(8,8) = 4*t160*m(9)+J(8)+m(8)*t160+4*t160*m(10)
  116.          me3s(6,6) = m(6)*t167+4*t167*m(7)+4*t167*m(8)+J(6)+4*t167*m(9)+
  117.      +4*t167*m(10)
  118.          me3s(10,6) = t183
  119.          me3s(10,10) = m(10)*t184+J(10)
  120.          me3s(4,2) = t192
  121.          me3s(9,9) = 4*t193*m(10)+J(9)+m(9)*t193
  122.          me3s(7,7) = 4*t198*m(9)+4*t198*m(10)+4*t198*m(8)+J(7)+m(7)*t198
  123.          me3s(6,3) = t212
  124.          me3s(3,2) = t218
  125.          me3s(10,3) = t144
  126.          me3s(9,5) = t224
  127.          me3s(7,5) = t230
  128.          me3s(10,7) = t236
  129.          me3s(1,4) = t242
  130.          me3s(4,10) = t150
  131.          me3s(5,6) = t53
  132.          me3s(6,1) = t248
  133.          me3s(6,2) = t254
  134.          me3s(10,2) = t260
  135.          me3s(8,9) = t88
  136.          me3s(6,7) = t109
  137.          me3s(10,8) = t266
  138.          me3s(1,5) = t274
  139.          me3s(9,10) = t115
  140.          me3s(5,7) = t230
  141.          me3s(5,4) = t280
  142.          me3s(7,8) = t24
  143.          me3s(7,4) = t286
  144.          me3s(9,4) = t138
  145.          me3s(1,3) = t159
  146.          me3s(8,10) = t266
  147.          me3s(6,8) = t292
  148.          me3s(9,1) = t298
  149.          me3s(1,6) = t248
  150.          me3s(5,8) = t304
  151.          me3s(5,3) = t310
  152.          me3s(7,9) = t9
  153.          me3s(7,3) = t316
  154.          me3s(9,3) = t15
  155.          me3s(2,7) = t322
  156.          me3s(1,2) = t331
  157.          me3s(4,4) = 4*t332*m(10)+4*t332*m(9)+4*t332*m(6)+4*t332*m(5)+4*
  158.      +t332*m(7)+J(4)+m(4)*t332+4*t332*m(8)
  159.          me3s(4,1) = t242
  160.          me3s(6,9) = t59
  161.          me3s(8,4) = t352
  162.          me3s(4,5) = t280
  163.          me3s(3,4) = t82
  164.          me3s(5,9) = t224
  165.          me3s(5,2) = t358
  166.          me3s(7,10) = t236
  167.          me3s(7,2) = t322
  168.          me3s(9,2) = t364
  169.          me3s(2,8) = t370
  170.          me3s(1,7) = t376
  171.          me3s(6,10) = t183
  172.          me3s(8,3) = t382
  173.          me3s(4,6) = t388
  174.          me3s(3,5) = t310
  175.          me3s(2,9) = t364
  176.          me3s(7,1) = t376
  177.          me3s(1,8) = t72
  178.          me3s(5,10) = t66
  179.          me3s(2,4) = t192
  180.          me3s(8,2) = t370
  181.          me3s(4,7) = t286
  182.          me3s(3,6) = t212
  183.          me3s(2,1) = t331
  184.          me3s(1,1) = 4*t389*m(8)+4*t389*m(10)+m(1)*t389+4*t389*m(6)+4*t3
  185.      +89*m(9)+4*t389*m(7)+4*t389*m(2)+J(1)+4*t389*m(5)+4*t389*m(4)+4*t38
  186.      +9*m(3)
  187.          me3s(2,10) = t260
  188.          me3s(1,9) = t298
  189.          me3s(10,1) = t417
  190.          me3s(2,5) = t358
  191.          me3s(4,8) = t352
  192.          me3s(3,7) = t316
  193.          me3s(8,5) = t304
  194.          me3s(2,3) = t218
  195.          me3s(1,10) = t417
  196.          me3s(5,1) = t274
  197.          me3s(2,6) = t254
  198.          me3s(3,8) = t382
  199.          me3s(8,6) = t292
  200.          me3s(6,4) = t388
  201.       t1 = -th(9)
  202.       t4 = 2*m(10)
  203.       t5 = m(9)+t4
  204.       t8 = r(7)*r(9)*sin(th(7)+t1)*t5
  205.       t14 = r(3)*r(9)*sin(th(3)+t1)*t5
  206.       t16 = -th(8)
  207.       t19 = 2*m(9)
  208.       t20 = m(8)+t4+t19
  209.       t23 = r(7)*r(8)*sin(th(7)+t16)*t20
  210.       t25 = -th(6)
  211.       t28 = 2*m(7)
  212.       t29 = 2*m(8)
  213.       t30 = m(6)+t19+t4+t28+t29
  214.       t33 = r(5)*r(6)*sin(th(5)+t25)*t30
  215.       t39 = r(6)*r(9)*sin(th(6)+t1)*t5
  216.       t41 = -th(10)
  217.       t46 = r(5)*m(10)*r(10)*sin(th(5)+t41)
  218.       t52 = r(1)*r(8)*sin(th(1)+t16)*t20
  219.       t54 = -th(4)
  220.       t57 = 2*m(5)
  221.       t58 = 2*m(6)
  222.       t59 = t29+m(4)+t19+t57+t28+t4+t58
  223.       t62 = r(3)*r(4)*sin(th(3)+t54)*t59
  224.       t68 = r(8)*r(9)*sin(th(8)+t1)*t5
  225.       t70 = -th(7)
  226.       t73 = t4+m(7)+t29+t19
  227.       t76 = r(6)*r(7)*sin(th(6)+t70)*t73
  228.       t82 = r(9)*m(10)*r(10)*sin(th(9)+t41)
  229.       t88 = r(4)*r(9)*sin(th(4)+t1)*t5
  230.       t94 = r(3)*m(10)*r(10)*sin(th(3)+t41)
  231.       t100 = r(4)*m(10)*r(10)*sin(th(4)+t41)
  232.       t102 = -th(3)
  233.       t105 = 2*m(4)
  234.       t106 = t57+t105+t4+t29+t28+m(3)+t19+t58
  235.       t109 = r(1)*r(3)*sin(th(1)+t102)*t106
  236.       t115 = r(6)*m(10)*r(10)*sin(th(6)+t41)
  237.       t121 = r(2)*r(4)*sin(th(2)+t54)*t59
  238.       t127 = r(3)*r(6)*sin(th(3)+t25)*t30
  239.       t133 = r(2)*r(3)*sin(th(2)+t102)*t106
  240.       t140 = r(5)*r(9)*sin(th(5)+t1)*t5
  241.       t146 = r(5)*r(7)*sin(th(5)+t70)*t73
  242.       t152 = r(7)*m(10)*r(10)*sin(th(7)+t41)
  243.       t158 = r(1)*r(4)*sin(th(1)+t54)*t59
  244.       t166 = r(1)*r(6)*sin(th(1)+t25)*t30
  245.       t172 = r(2)*r(6)*sin(th(2)+t25)*t30
  246.       t178 = r(2)*m(10)*r(10)*sin(th(2)+t41)
  247.       t186 = r(8)*m(10)*r(10)*sin(th(8)+t41)
  248.       t188 = -th(5)
  249.       t191 = t58+t29+t19+t28+m(5)+t4
  250.       t194 = r(1)*r(5)*sin(th(1)+t188)*t191
  251.       t202 = r(4)*r(5)*sin(th(4)+t188)*t191
  252.       t209 = r(4)*r(7)*sin(th(4)+t70)*t73
  253.       t218 = r(6)*r(8)*sin(th(6)+t16)*t20
  254.       t224 = r(1)*r(9)*sin(th(1)+t1)*t5
  255.       t231 = r(5)*r(8)*sin(th(5)+t16)*t20
  256.       t237 = r(3)*r(5)*sin(th(3)+t188)*t191
  257.       t244 = r(3)*r(7)*sin(th(3)+t70)*t73
  258.       t251 = r(2)*r(7)*sin(th(2)+t70)*t73
  259.       t260 = r(1)*r(2)*sin(th(1)-th(2))*(2*m(3)+m(2)+t105+t28+t4+t57+t58
  260.      ++t19+t29)
  261.       t268 = r(4)*r(8)*sin(th(4)+t16)*t20
  262.       t277 = r(2)*r(5)*sin(th(2)+t188)*t191
  263.       t285 = r(2)*r(9)*sin(th(2)+t1)*t5
  264.       t291 = r(2)*r(8)*sin(th(2)+t16)*t20
  265.       t297 = r(1)*r(7)*sin(th(1)+t70)*t73
  266.       t304 = r(3)*r(8)*sin(th(3)+t16)*t20
  267.       t310 = r(4)*r(6)*sin(th(4)+t25)*t30
  268.       t328 = r(1)*m(10)*r(10)*sin(th(1)+t41)
  269.          cc3s(9,7) = -2*t8
  270.          cc3s(3,9) = 2*t14
  271.          cc3s(8,7) = -2*t23
  272.          cc3s(2,2) = 0
  273.          cc3s(6,5) = -2*t33
  274.          cc3s(9,6) = -2*t39
  275.          cc3s(10,5) = -2*t46
  276.          cc3s(8,1) = -2*t52
  277.          cc3s(4,3) = -2*t62
  278.          cc3s(9,8) = -2*t68
  279.          cc3s(5,5) = 0
  280.          cc3s(7,6) = -2*t76
  281.          cc3s(10,9) = -2*t82
  282.          cc3s(3,3) = 0
  283.          cc3s(4,9) = 2*t88
  284.          cc3s(3,10) = 2*t94
  285.          cc3s(10,4) = -2*t100
  286.          cc3s(3,1) = -2*t109
  287.          cc3s(8,8) = 0
  288.          cc3s(6,6) = 0
  289.          cc3s(10,6) = -2*t115
  290.          cc3s(10,10) = 0
  291.          cc3s(4,2) = -2*t121
  292.          cc3s(9,9) = 0
  293.          cc3s(7,7) = 0
  294.          cc3s(6,3) = -2*t127
  295.          cc3s(3,2) = -2*t133
  296.          cc3s(10,3) = -2*t94
  297.          cc3s(9,5) = -2*t140
  298.          cc3s(7,5) = -2*t146
  299.          cc3s(10,7) = -2*t152
  300.          cc3s(1,4) = 2*t158
  301.          cc3s(4,10) = 2*t100
  302.          cc3s(5,6) = 2*t33
  303.          cc3s(6,1) = -2*t166
  304.          cc3s(6,2) = -2*t172
  305.          cc3s(10,2) = -2*t178
  306.          cc3s(8,9) = 2*t68
  307.          cc3s(6,7) = 2*t76
  308.          cc3s(10,8) = -2*t186
  309.          cc3s(1,5) = 2*t194
  310.          cc3s(9,10) = 2*t82
  311.          cc3s(5,7) = 2*t146
  312.          cc3s(5,4) = -2*t202
  313.          cc3s(7,8) = 2*t23
  314.          cc3s(7,4) = -2*t209
  315.          cc3s(9,4) = -2*t88
  316.          cc3s(1,3) = 2*t109
  317.          cc3s(8,10) = 2*t186
  318.          cc3s(6,8) = 2*t218
  319.          cc3s(9,1) = -2*t224
  320.          cc3s(1,6) = 2*t166
  321.          cc3s(5,8) = 2*t231
  322.          cc3s(5,3) = -2*t237
  323.          cc3s(7,9) = 2*t8
  324.          cc3s(7,3) = -2*t244
  325.          cc3s(9,3) = -2*t14
  326.          cc3s(2,7) = 2*t251
  327.          cc3s(1,2) = 2*t260
  328.          cc3s(4,4) = 0
  329.          cc3s(4,1) = -2*t158
  330.          cc3s(6,9) = 2*t39
  331.          cc3s(8,4) = -2*t268
  332.          cc3s(4,5) = 2*t202
  333.          cc3s(3,4) = 2*t62
  334.          cc3s(5,9) = 2*t140
  335.          cc3s(5,2) = -2*t277
  336.          cc3s(7,10) = 2*t152
  337.          cc3s(7,2) = -2*t251
  338.          cc3s(9,2) = -2*t285
  339.          cc3s(2,8) = 2*t291
  340.          cc3s(1,7) = 2*t297
  341.          cc3s(6,10) = 2*t115
  342.          cc3s(8,3) = -2*t304
  343.          cc3s(4,6) = 2*t310
  344.          cc3s(3,5) = 2*t237
  345.          cc3s(2,9) = 2*t285
  346.          cc3s(7,1) = -2*t297
  347.          cc3s(1,8) = 2*t52
  348.          cc3s(5,10) = 2*t46
  349.          cc3s(2,4) = 2*t121
  350.          cc3s(8,2) = -2*t291
  351.          cc3s(4,7) = 2*t209
  352.          cc3s(3,6) = 2*t127
  353.          cc3s(2,1) = -2*t260
  354.          cc3s(1,1) = 0
  355.          cc3s(2,10) = 2*t178
  356.          cc3s(1,9) = 2*t224
  357.          cc3s(10,1) = -2*t328
  358.          cc3s(2,5) = 2*t277
  359.          cc3s(4,8) = 2*t268
  360.          cc3s(3,7) = 2*t244
  361.          cc3s(8,5) = -2*t231
  362.          cc3s(2,3) = 2*t133
  363.          cc3s(1,10) = 2*t328
  364.          cc3s(5,1) = -2*t194
  365.          cc3s(2,6) = 2*t172
  366.          cc3s(3,8) = 2*t304
  367.          cc3s(8,6) = -2*t218
  368.          cc3s(6,4) = -2*t310
  369.       t2 = 2*m(10)
  370.       t3 = 2*m(9)
  371.       t9 = 2*m(5)
  372.       t10 = 2*m(4)
  373.       t11 = 2*m(8)
  374.       t12 = 2*m(7)
  375.       t13 = 2*m(6)
  376.       t39 = 2*m(3)
  377.          const(8,1) = g*cos(th(8))*r(8)*(m(8)+t2+t3)
  378.          const(3,1) = g*r(3)*cos(th(3))*(t9+t10+t2+t11+t12+m(3)+t3+t13)
  379.          const(6,1) = g*r(6)*cos(th(6))*(m(6)+t3+t2+t12+t11)
  380.          const(9,1) = g*r(9)*cos(th(9))*(m(9)+t2)
  381.          const(4,1) = g*r(4)*cos(th(4))*(t11+m(4)+t3+t9+t12+t2+t13)
  382.          const(7,1) = g*r(7)*cos(th(7))*(t2+m(7)+t11+t3)
  383.          const(2,1) = g*r(2)*cos(th(2))*(t39+m(2)+t10+t12+t2+t9+t13+t3+t
  384.      +11)
  385.          const(1,1) = g*r(1)*cos(th(1))*(m(1)+t3+2*m(2)+t10+t39+t11+t12+
  386.      +t13+t9+t2)
  387.          const(10,1) = m(10)*g*r(10)*cos(th(10))
  388.          const(5,1) = g*r(5)*cos(th(5))*(t13+t11+t3+t12+m(5)+t2)
  389. c         
  390.         do 1000, i =1,n ,1
  391.           ydot(i) = th(i+n)
  392.  1000   continue
  393. c       
  394. c         
  395.         do 1001, i =1,n ,1
  396. c           
  397.           do 1002, k =1,n ,1
  398.             Const(i,1) =  Const(i,1)+CC3S(i,k)*(th(k+n)**2)
  399.  1002     continue
  400. c         
  401.            Const(i,1) = -Const(i,1)
  402.  1001   continue
  403. c       
  404. c        we must solve  M z =const 
  405. c        which gives ydot((n+1)..2*n) 
  406.         call dlslv(me3s,n,n,Const,n,1,w, rcond,ierr,1)
  407.         if (ierr.ne.0) then
  408.           write(6,2000) 
  409.  2000     format('Matrice mal conditionnee')
  410.         endif
  411. c         
  412.         do 1003, i =1,n ,1
  413.           ydot(n+i) = const(i,1)
  414.  1003   continue
  415. c       
  416.         return
  417.       end
  418.